#include <Wire.h>
// Wire library uses 7-bit addresses and automatically
// sets the r/w bit
static const uint8_t HMC5883L = 0x1E;
static const uint8_t CONFIG_A = 0x00;
static   const uint8_t BIT_MA1 = 0x40;
static   const uint8_t BIT_MA0 = 0x20;
static   const uint8_t BIT_DO2 = 0x10;
static   const uint8_t BIT_DO1 = 0x08;
static   const uint8_t BIT_DO0 = 0x04;
static   const uint8_t BIT_MS1 = 0x02;
static   const uint8_t BIT_MS0 = 0x01;
static   const uint8_t BIT_OUTPUT_0_75 = 0x00;
static   const uint8_t BIT_OUTPUT_1_5  = BIT_DO0;
static   const uint8_t BIT_OUTPUT_3    = BIT_DO1;
static   const uint8_t BIT_OUTPUT_7_5  = (BIT_DO1 | BIT_DO0);
static   const uint8_t BIT_OUTPUT_15   = BIT_DO2;
static   const uint8_t BIT_OUTPUT_30   = (BIT_DO2 | BIT_DO0);
static   const uint8_t BIT_OUTPUT_75   = (BIT_DO2 | BIT_DO1);
static   const uint8_t BIT_MEAS_NORM     = 0x00;
static   const uint8_t BIT_MEAS_POS_BIAS = BIT_MS0;
static   const uint8_t BIT_MEAS_NEG_BIAS = BIT_MS1;
static   const uint8_t BIT_MEAS_AVG_1 = 0x00;
static   const uint8_t BIT_MEAS_AVG_2 = BIT_MA0;
static   const uint8_t BIT_MEAS_AVG_4 = BIT_MA1;
static   const uint8_t BIT_MEAS_AVG_8 = (BIT_MA1 | BIT_MA0);
static const uint8_t CONFIG_B = 0x01;
static   const uint8_t BIT_GN2 = 0x80;
static   const uint8_t BIT_GN1 = 0x40;
static   const uint8_t BIT_GN0 = 0x20;
static   const uint8_t BIT_GAIN_1370 = 0x00;
static   const uint8_t BIT_GAIN_1090 = BIT_GN0;
static   const uint8_t BIT_GAIN_820  = BIT_GN1;
static   const uint8_t BIT_GAIN_660  = (BIT_GN1 | BIT_GN0);
static   const uint8_t BIT_GAIN_440  = BIT_GN2;
static   const uint8_t BIT_GAIN_390  = (BIT_GN2 | BIT_GN0);
static   const uint8_t BIT_GAIN_330  = (BIT_GN2 | BIT_GN1);
static   const uint8_t BIT_GAIN_230  = (BIT_GN2 | BIT_GN1 | BIT_GN0);
static const uint8_t MODE = 0x02;
static   const uint8_t BIT_MD1 = 0x02;
static   const uint8_t BIT_MD0 = 0x01;
static   const uint8_t BIT_MODE_CONTINUOUS = 0x00;
static   const uint8_t BIT_MODE_SINGLE     = BIT_MD0;
static   const uint8_t BIT_MODE_IDLE       = (BIT_MD1 | BIT_MD0);
static const uint8_t XMSB = 0x03;
static const uint8_t XLSB = 0x04;
static const uint8_t ZMSB = 0x05;
static const uint8_t ZLSB = 0x06;
static const uint8_t YMSB = 0x07;
static const uint8_t YLSB = 0x08;
static const uint8_t STATUS = 0x09;
static   const uint8_t BIT_LOCK = 0x02;
static   const uint8_t BIT_RDY  = 0x01;
static const uint8_t ID_A = 0x0a;
static const uint8_t ID_B = 0x0b;
static const uint8_t ID_C = 0x0c;

void writeHMC(uint8_t reg, uint8_t val)
{
  static const uint8_t writeAddr = HMC5883L << 1;
  Wire.beginTransmission(HMC5883L);
  Wire.send(reg);
  Wire.send(val);
  Wire.endTransmission();
  // print the message, stuck here to make sure I was matching
  // the datasheet.
  Serial.print("0x");
  Serial.print(writeAddr, HEX);
  Serial.print(" 0x");
  Serial.print(reg, HEX);
  Serial.print(" 0x");
  Serial.println(val, HEX);
}

void setup()
{
  Serial.begin(9600);
  Wire.begin();
  // following self-test operation example in datasheet
  // example in datasheet sets config A to 0x71, which is:
  uint8_t careg = BIT_MEAS_AVG_8 | BIT_OUTPUT_15 | BIT_MEAS_POS_BIAS;
  // example in datasheet sets config B to 0xA0, which is:
  uint8_t cbreg = BIT_GAIN_390;
  Serial.print("CONFIG_A ");
  writeHMC(CONFIG_A, careg);
  // following the example, we send each register change separately
  Serial.print("CONFIG_B ");
  writeHMC(CONFIG_B, cbreg);
  // another separate update...
  Serial.print("MODE ");
  writeHMC(MODE, BIT_MODE_CONTINUOUS);
  Serial.println("done initializing");
  // wait 6ms rather than monitor unavailable hardware interrupt
  delay(6);
}

void loop()
{
  int i;
  // 6 bytes gets us the LSB and MSB of the XYZ data
  Wire.requestFrom(HMC5883L, (uint8_t)6);
  // store the data in a signed integer quantity
  int16_t acc[3];
  // pointer to use to store the data, stored in reverse for
  // endian-ness.
  byte *p = (byte*)&(acc[2]);
  *p++; // last byte
  // wait for 6 bytes to be available
  while (Wire.available() < 6) { }
  for (int i = 0; i < 6; i++)
  {
    *p = Wire.receive();
    p--; // advance the pointer to the next 8 bits
  }
  Serial.print(acc[2]); // finally, print the quantities X Y Z
  Serial.print(" ");
  Serial.print(acc[0]);
  Serial.print(" ");
  Serial.print(acc[1]);
  int ok = 1;
  // check the values and report.
  if ((acc[2] < 243) || (acc[2] > 575))
  {
    Serial.print(" bad-X");
    ok = 0;
  }
  if ((acc[0] < 243) || (acc[0] > 575))
  {
    Serial.print(" bad-Y");
    ok = 0;
  }
  if ((acc[1] < 243) || (acc[1] > 575))
  {
    Serial.print(" bad-Z");
    ok = 0;
  }
  if (ok)
  {
    Serial.print(" OK");
  }
  Serial.println("");
    // point to the first data register
  Wire.beginTransmission(HMC5883L);
  Wire.send(XMSB);
  Wire.endTransmission();
  delay(67); // delay a bit to avoid flooding
}
